#include <Arduino.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <LobotServoController.h>
#include <micro_ros_platformio.h>

LobotServoController myse;

#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>

#include <std_msgs/msg/int32.h>
#include<std_msgs/msg/float32.h>
#include<std_msgs/msg/string.h>

rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t timer;

//rcl_subscription_t subscriber;// 声明话题订阅者
rcl_subscription_t subscriber_int32;
rcl_subscription_t subscriber_float32;
std_msgs__msg__Int32 sub_msgINT32;//声明消息文件int
std_msgs__msg__Float32 sub_msgFLOAT32;//声明消息文件float


float Lshoulder=43.9;
float Llong=150.0;
float Lfoot=153.7;
float AngTilt=atan(33.5/150);

float body_height=0.0;

int L1S1=5,L1S2=6,L1S3=7,L2S1=2,L2S2=1,L2S3=0,L3S1=10,L3S2=9,L3S3=8,L4S1=13,L4S2=14,L4S3=15;
int pL1S1=1500,pL1S2=500,pL1S3=500,pL2S1=1500,pL2S2=500,pL2S3=500,pL3S1=1500,pL3S2=2500,pL3S3=2500,pL4S1=1500,pL4S2=2500,pL4S3=2500;
int eL1S1=0,eL1S2=0,eL1S3=0,eL2S1=0,eL2S2=0,eL2S3=0,eL3S1=0,eL3S2=0,eL3S3=0,eL4S1=0,eL4S2=0,eL4S3=0;//pwm误差

void set12Servo(int Time){
  myse.moveServos(
    12,
    Time,
    L1S1,pL1S1-eL1S1,
    L1S2,pL1S2-eL1S2,
    L1S3,pL1S3-eL1S3,
    L2S1,pL2S1-eL2S1,
    L2S2,pL2S2-eL2S2,
    L2S3,pL2S3-eL2S3,
    L3S1,pL3S1-eL3S1,
    L3S2,pL3S2-eL3S2,
    L3S3,pL3S3-eL3S3,
    L4S1,pL4S1-eL4S1,
    L4S2,pL4S2-eL4S2,
    L4S3,pL4S3-eL4S3);
  delay(Time);
}

void callback_subscription_(const void *msgin){
  const std_msgs__msg__Int32 *msg = (const std_msgs__msg__Int32 *)msgin;
  if (msg->data == 1)
  {
    set12Servo(1000);
  }
}

void callback_subscription2_(const void *msgin){
  const std_msgs__msg__Float32 *msg=(const std_msgs__msg__Float32 *)msgin;
  body_height=msg->data;
}


void setup() {
  // put your setup code here, to run once:

  Serial.begin(9600);
  Serial2.begin(115200);
  set_microros_serial_transports(Serial2);//串口Serial2进行MicroROS通信
  delay(2000);
   // 初始化内存分配器
  allocator = rcl_get_default_allocator();
  // 创建初始化选项
  rclc_support_init(&support, 0, NULL, &allocator);
  // 创建节点robot_dog6
  rclc_node_init_default(&node,"robot_dog6","",&support);
  // 订阅者初始化
  rclc_subscription_init_default(
    &subscriber_int32,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "robot_control");
  rclc_subscription_init_default(
    &subscriber_float32,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32),
    "data_input");
  // 创建执行器
  rclc_executor_init(&executor,&support.context,1,&allocator);
  // 为执行器添加一个订阅者
  rclc_executor_add_subscription(&executor, &subscriber_int32, &sub_msgINT32, &callback_subscription_, ON_NEW_DATA);
  rclc_executor_add_subscription(&executor, &subscriber_float32, &sub_msgFLOAT32, &callback_subscription2_, ON_NEW_DATA);
  delay(5000);
}

void loop() {
  // put your main code here, to run repeatedly:
  //Serial.println(1);
  //set12Servo(1000);
  // 循环处理数据
  rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
}

void legAngle(int i, float X, float Y, float Z){
  //计算shoulder摆动角AngShoulder
  float AngYZ=acos(Y/sqrt(Y*Y+Z*Z));
  if(Z<0){
    AngYZ=2*PI-AngYZ;
  }
  float AngShoulder;
  if (i==1||i==2)
  {
    AngShoulder=acos(Lshoulder/sqrt(Y*Y+Z*Z))+AngYZ-2*PI;
  }
  else
  {
    AngShoulder=AngYZ-acos(Lshoulder/sqrt(Y*Y+Z*Z));
    // if (AngShoulder>PI)
    // {
    //   AngShoulder=AngShoulder-2*PI;
    // }
  }
  //计算腿部二维坐标
  float Lyz=-sqrt(Y*Y+Z*Z-Lshoulder*Lshoulder);
  float c3=(X*X+Lyz*Lyz-Llong*Llong-Lfoot*Lfoot)/(2*Llong*Lfoot);
  float ca=(Llong+Lfoot*c3)/sqrt(Lyz*Lyz+X*X);
  float Angxyz=acos(X/sqrt(Lyz*Lyz+X*X));
  if(Lyz<0){
    Angxyz=-Angxyz;
  }
  float AngLong=Angxyz-acos(ca);//计算Long摆动角AngLong
  float AngFoot=acos(c3)-AngTilt;//计算Foot摆动角AngFoot
  //
  //转舵机角
  float S1, S2, S3;
  switch (i)
  {
  case 1:
    S1=PI/2.0+AngShoulder;
    S2=-AngLong;
    S3=AngFoot+PI;
    //计算单腿三个舵机角度
    pL1S1=round(S1*2000.0/PI+500.0);
    if (pL1S1<500)
    {
      pL1S1=500;
    }
    else if (pL1S1>2500)
    {
      pL1S1=2500;
    }
    //
    pL1S2=round(S2*2000.0/PI+500.0);
    if (pL1S2<500)
    {
      pL1S2=500;
    }
    else if (pL1S2>2500)
    {
      pL1S2=2500;
    }
    //
    pL1S3=round(S3*2000.0/PI+500.0);
    if (pL1S3<500)
    {
      pL1S3=500;
    }
    else if (pL1S3>2500)
    {
      pL1S3=2500;
    }
    break;
  case 2:
    S1=PI/2.0+AngShoulder;
    S2=-AngLong;
    S3=AngFoot+PI;
        //计算单腿三个舵机角度
    pL2S1=round(S1*2000.0/PI+500.0);
    if (pL2S1<500)
    {
      pL2S1=500;
    }
    else if (pL2S1>2500)
    {
      pL2S1=2500;
    }
    //
    pL2S2=round(S2*2000.0/PI+500.0);
    if (pL2S2<500)
    {
      pL2S2=500;
    }
    else if (pL2S2>2500)
    {
      pL2S2=2500;
    }
    //
    pL2S3=round(S3*2000.0/PI+500.0);
    if (pL2S3<500)
    {
      pL2S3=500;
    }
    else if (pL2S3>2500)
    {
      pL2S3=2500;
    }
    break;
  case 3:
    S1=PI-AngShoulder+PI/2.0;
    S2=PI+AngLong;
    S3=-AngFoot+PI/2.0;
        //计算单腿三个舵机角度
    pL3S1=round(S1*2000.0/PI+500.0);
    if (pL3S1<500)
    {
      pL3S1=500;
    }
    else if (pL3S1>2500)
    {
      pL3S1=2500;
    }
    //
    pL3S2=round(S2*2000.0/PI+500.0);
    if (pL3S2<500)
    {
      pL3S2=500;
    }
    else if (pL3S2>2500)
    {
      pL3S2=2500;
    }
    //
    pL3S3=round(S3*2000.0/PI+500.0);
    if (pL3S3<500)
    {
      pL3S3=500;
    }
    else if (pL3S3>2500)
    {
      pL3S3=2500;
    }
    break;
  case 4:
    S1=PI-AngShoulder+PI/2.0;
    S2=PI+AngLong;
    S3=-AngFoot+PI/2.0;
        //计算单腿三个舵机角度
    pL4S1=round(S1*2000.0/PI+500.0);
    if (pL4S1<500)
    {
      pL4S1=500;
    }
    else if (pL4S1>2500)
    {
      pL4S1=2500;
    }
    //
    pL4S2=round(S2*2000.0/PI+500.0);
    if (pL4S2<500)
    {
      pL4S2=500;
    }
    else if (pL4S2>2500)
    {
      pL4S2=2500;
    }
    //
    pL4S3=round(S3*2000.0/PI+500.0);
    if (pL4S3<500)
    {
      pL4S3=500;
    }
    else if (pL4S3>2500)
    {
      pL4S3=2500;
    }
    break;
  default:
    break;
  }
}
